#include <Wire.h>
#include "LSM303.h"
#include <SD.h>

// SD
const int chipSelect = 10;
const char * output_file = "combined.txt";
// Serial communication between compass and Arduino
LSM303 compass;

// Sensor numbering
const int sens0 = 0;
const int sens1 = 1;
const int sens2 = 2;
const int sens3 = 3;

// Compass data (6) and analog data (4)
int data[10] = {0,0,0,0,0,0,0,0,0,0};

// Data time stamp (in milliseconds)
unsigned long time = 0; // Time of last/current sample
unsigned long prev_time = 0; // Time of last mag sample
const unsigned long sample_interval = 1000; // 1000 Hz
const unsigned long mag_sample_interval = 10000; // 100 Hz

// At the start of each line of data
const byte sentinel = B00100100; // '$' when line doesn't have mag data
const byte mag_sentinel= B00100011; // '#' when line has all data

void setup() {
  Serial.begin(115200);
  //output CS or SS from SD card
  pinMode(chipSelect, OUTPUT);
  
  if(SD.begin(chipSelect))
    Serial.println("SD card successfully initialized.");
  else
    Serial.println("SD card failed to initialize.");

  delay(2000);
  // Compass setup
  Wire.begin();
  compass.init(); //Turns on LSM303DLH
  //compass.enableDefault();
  compass.writeAccReg(LSM303_CTRL_REG1_A, 0x3F); // Acc output data rate (ODR) = 1000 Hz
  compass.writeAccReg(LSM303_CTRL_REG4_A, 0x30); // Set accelerometer scale to +/-8 g
  compass.writeMagReg(LSM303_MR_REG_M, 0x00);    // Default
  compass.writeMagReg(LSM303_CRA_REG_M, 0x18);   // Mag output data rate (ODR) = 75 Hz (max)
  delay(1000);
  
  // adjust the prescale for the analog inputs (http://arduino.cc/forum/index.php/topic,6549.0.html)
  // defines for setting and clearing register bit
  #ifndef cbi
  #define cbi(sfr, bit) (_SFR_BYTE(sfr) &= ~_BV(bit))
  #endif
  #ifndef sbi
  #define sbi(sfr, bit) (_SFR_BYTE(sfr) |= _BV(bit))
  #endif
  // set prescale to 16
  sbi(ADCSRA,ADPS2);
  cbi(ADCSRA,ADPS1);
  cbi(ADCSRA,ADPS0);
  
//  // Need to modify the daughterboard for this to work
//  Serial.print("Waiting for start command (!): ");
//  char c = '\0';
//  while (c != '!')
//  {
//    if (Serial.available() > 0)
//      c = Serial.read();
//    else
//      delay(1000);
//  }
}

void loop() {
  File dataFile = SD.open(output_file, FILE_WRITE);
  if (dataFile)
    Serial.print("Successfully opened ");
  else
    Serial.print("Failed to open ");
  Serial.println(output_file);
  
  if (dataFile.write(B00000001))
    Serial.println("Successfully writing to SD card.");
  else
    Serial.println("Cannot write to SD card.");
    
  // Main loop
  unsigned int loop_number = 0;
  while (1)
  {
    if ((micros() - time) > sample_interval)
    {
      // Collect data as close to time stamp as possible
      time = micros();
      compass.readAcc();
      data[0] = (unsigned int)analogRead(sens0);
      data[1] = (unsigned int)analogRead(sens1);
      data[2] = (unsigned int)analogRead(sens2);
      data[3] = (unsigned int)analogRead(sens3);
      data[4] = (int)compass.a.x;
      data[5] = (int)compass.a.y;
      data[6] = (int)compass.a.z;
      if ((time - prev_time) > mag_sample_interval)
      {
        compass.readMag();
        data[7] = (int)compass.m.x;
        data[8] = (int)compass.m.y;
        data[9] = (int)compass.m.z;
        // Write data to the SD card if the file was successfully opened
        if (dataFile)
        {
          dataFile.write(mag_sentinel);
          dataFile.write((byte *)(&time),4);
          dataFile.write((byte *)data,20);
        }
        // Send data over the radio
        Serial.write(mag_sentinel);
        Serial.write((byte *)(&time),4);
        Serial.write((byte *)data,20);
        prev_time = time;
      }
      else
      {
        // Write data to the SD card if the file was successfully opened
        if (dataFile)
        {
          dataFile.write(sentinel);
          dataFile.write((byte *)(&time),4);
          dataFile.write((byte *)data,14);
        }
        // Send data over the radio
        Serial.write(sentinel);
        Serial.write((byte *)(&time),4);
        Serial.write((byte *)data,14);
      }
      
      ++loop_number;
      if (loop_number >= 100)
      {
        dataFile.flush(); 
        loop_number = 0;
      }
    }
  }
  dataFile.close();
}
